#include <Bender.h>

int motorSpeed;

int lastDirection;

int buttonState;

int activeHallEffect;
int lastHallEffect;

void buttonPressed(void){
  buttonState = 1;
}

void initBender(void) { 
  pinMode(MOTOR_ENABLE, OUTPUT);
  pinMode(MOTOR_CLOCKWISE, OUTPUT);
  pinMode(MOTOR_COUNTER_CLOCKWISE, OUTPUT);
  digitalWrite(MOTOR_ENABLE, LOW);   //Default motor enable to LOW
  
  pinMode(HALL_GS, INPUT);
  pinMode(HALL1, INPUT);
  pinMode(HALL2, INPUT);
  pinMode(HALL3, INPUT);
  
  pinMode(BIG_RED_BUTTON, INPUT);
  attachInterrupt(2, buttonPressed, RISING);
  buttonState = 0;
  
  pinMode(VALVE_ENABLE, OUTPUT);
  pinMode(VALVE1, OUTPUT);
  pinMode(VALVE2, OUTPUT);
  pinMode(VALVE3, OUTPUT);
  // Disable valves by default
  digitalWrite(VALVE_ENABLE, HIGH);
  
  pinMode(LED_ENABLE, OUTPUT);
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(LED3, OUTPUT);
  // Disable LED by default
  digitalWrite(LED_ENABLE, HIGH);
}

void MotorCtrl(int motorDirection, int motorCtrlSpeed){
  if(motorCtrlSpeed > 0){
    digitalWrite(MOTOR_ENABLE, HIGH);
  }
  else{
    digitalWrite(MOTOR_ENABLE, LOW);
    Serial.println("Motor Off");
    return;
  }
  
  if(motorDirection == CW){
    Serial.print("Turning CW at ");
    Serial.println(motorCtrlSpeed);
    analogWrite(MOTOR_CLOCKWISE, motorCtrlSpeed);
    digitalWrite(MOTOR_COUNTER_CLOCKWISE, LOW);
  }
  else if(motorDirection == CCW){
    Serial.print("Turning CCW at ");
    Serial.println(motorCtrlSpeed);
    digitalWrite(MOTOR_CLOCKWISE, LOW);
    analogWrite(MOTOR_COUNTER_CLOCKWISE, motorCtrlSpeed);
  }
}

void openValve(int valve) {
  digitalWrite(VALVE_ENABLE, LOW);
  Serial.print("Opening Valve ");
  Serial.println((valve & 0x07));
  digitalWrite(VALVE1, valve & 0x01 ? HIGH : LOW);
  digitalWrite(VALVE2, valve & 0x02 ? HIGH : LOW);
  digitalWrite(VALVE3, valve & 0x04 ? HIGH : LOW);
}
void closeValves() {
  Serial.println("Closing valves");
  digitalWrite(VALVE_ENABLE, HIGH);
}

void turnOnLED(int led, int state) {
  digitalWrite(LED_ENABLE, HIGH);
  digitalWrite(LED1, led & 0x01 ? HIGH : LOW);
  digitalWrite(LED2, led & 0x02 ? HIGH : LOW);
  digitalWrite(LED3, led & 0x04 ? HIGH : LOW);
  digitalWrite(LED_ENABLE, state);
} 

